// REQUIRES sylvester.js; this can be found in renderengine/libs

/*********************************************************************
This code implements the Dormand-Prince ODE solver as described in 
http://en.wikipedia.org/wiki/Dormand–Prince_method. This is the method
used in MATLABs ode45 method. This code is also designed to 
implement an adaptive stepsize control as described in chapter 16 of
"Numerical Recipes in C: The Art of Scientific Computing." 

NOTE: this code requires sylvester.js, and in the documentation 
sylvester vectors are called sVec, and sylvester marices are called
sMat.

*********************************************************************/

function ode45(f, x0, u, t0, timeLength, eps, h0){
    // ODE45 - Numerically solve an ODE using Dormand-Prince Method
    //   @input 
    //     f: User defined function that takes three
    //       arguments--time, sVec state, and sVec input--and outputs 
    //       a sVec of the state.
    //     x0: A sVec representing the initial state
    //     u: A sVec representing the constant input over the time frame.
    //     t0: The initial time of the simulation
    //     timeLength: the length of time for the simulation to run
    //     eps: the desired precision of the output
    //     h0: OPTIONAL initial stepsize for algorithm
    //   @output
    //     x: sVec of final state at time t0+timeLength
    if (!eps){// Make eps an optional argument with default value eps = 0.000001
        eps = 0.000001
    }
    if (!h0){// Make h0 an optional argument with default value h = 0.001
        h = 0.001
    }
    else{
	h = h0
    }
    // Define Dormand-Prince Butcher Tableau for calculating slopes
    var a = $M([
	[         0,           0,          0,        0,           0,     0, 0],
	[       1/5,           0,          0,        0,           0,     0, 0],
	[      3/40,        9/40,          0,        0,           0,     0, 0],
	[     44/45,      -56/15,       32/9,        0,           0,     0, 0],
	[19372/6561, -25360/2187, 64448/6561, -212/729,           0,     0, 0],
	[ 9017/3168,     -355/33, 46732/5247,   49/176, -5103/18656,     0, 0],
	[    35/384,           0,   500/1113,  125/192,  -2187/6784, 11/84, 0]
    ])
    var b = $V(
	[5179/57600, 0, 7571/16695, 393/640, -92097/339200, 187/2100, 1/40]
    )
    var c = $V(
	[0, 1/5, 3/10, 4/5, 8/9, 1, 1]
    )
    var k = new Array(7)  // Slope estimates
    var del0, del1, ratio // Desired/Actual error terms
    var S = 0.9           // Numerical Safety Factor
    var x = x0;           // State
    var x_4, x_5;         // 4th and 5th order estimates
    var t = t0;           // Time
    var k_len = k.length;
    while(t < t0 + timeLength){
	if(t + h > timeLength){
	    // End simulation at t0 + timeLength
	    h = timeLength - t;
	}
	// .x() aliases .multiply()
	x_5 = x.dup()
	for(var i = 1; i <= k_len; i++){
	    x_4 = x.dup()
	    for(var j = 1; j <= i-1; j++){
		x_4 = x_4.add(k[j-1].x(a.e(i,j)*h))
	    }
	    k[i-1] = f(t+c.e(i)*h, x_4, u)
	    x_5 = x_5.add(k[i-1].x(b.e(i)*h))
	}

	inc = x_5.subtract(x) // Step size, = h*dxdt
	// Desired error precision sVec
	// del0 = eps*(|x(i)| + |h*dxdt|)
	del0 = x_5.map(function(el,i){
	    return Math.abs(el) + Math.abs(inc.e(i))
	}).x(eps)
	// Error estimate sVec
	del1 = x_5.subtract(x_4)
	// max() returns the element with the largest absolute value
	ratio = 1 / Math.abs(del1.map(function(el,i){
	    return el/del0.e(i)
	}).max())
	ratio = Math.min(ratio, 1000000)
	if(ratio <= 1){
	    // Error too large; decrease h and try again
	    h = S*h*Math.pow(ratio,0.25)
	}
	else{
	    // Move on to the next step and increase h for efficiency
	    t += h
	    x = x.add(inc)
	    h = S*h*Math.pow(ratio,0.2)
	}
    }
    return x
}

function ode45_all(f, x0, u, t0, timeLength, eps, h0){
    // Returns array of all values for t and x
    if (!h0){
        h = 0.001
    }
    else{
	h = h0
    }
    var a = $M([
	[         0,           0,          0,        0,           0,     0, 0],
	[       1/5,           0,          0,        0,           0,     0, 0],
	[      3/40,        9/40,          0,        0,           0,     0, 0],
	[     44/45,      -56/15,       32/9,        0,           0,     0, 0],
	[19372/6561, -25360/2187, 64448/6561, -212/729,           0,     0, 0],
	[ 9017/3168,     -355/33, 46732/5247,   49/176, -5103/18656,     0, 0],
	[    35/384,           0,   500/1113,  125/192,  -2187/6784, 11/84, 0]
    ])
    var b = $V(
	[5179/57600, 0, 7571/16695, 393/640, -92097/339200, 187/2100, 1/40]
    )
    var c = $V(
	[0, 1/5, 3/10, 4/5, 8/9, 1, 1]
    )
    var k = new Array(7) 
    var del0, del1, ratio
    var S = 0.9 // Numerical Safety Factor
    var vals = new Array()
    vals[0] = [t0, x0.e(1)]
    var x = x0;
    var x_4, x_5;
    var idx = 0;
    var t = t0;
    var k_len = k.length;
    while(t < t0 + timeLength){
	if(t + h > timeLength){
	    h = timeLength - t;
	}
	// .x() aliases .multiply()
	x_5 = x.dup()
	for(var i = 1; i <= k_len; i++){
	    x_4 = x.dup()
	    for(var j = 1; j <= i-1; j++){
		x_4 = x_4.add(k[j-1].x(a.e(i,j)*h))
	    }
	    k[i-1] = f(t+c.e(i)*h, x_4, u)
	    x_5 = x_5.add(k[i-1].x(b.e(i)*h))
	}
	inc = x_5.subtract(x)
	//del0 = eps*(|x(i)| + |h*dxdt|)
	del0 = x_5.map(function(el,i){
	    return Math.abs(el) + Math.abs(inc.e(i))
	}).x(eps)
	del1 = x_5.subtract(x_4)
	// max() returns the element with the largest absolute value
	ratio = 1 / Math.abs(del1.map(function(el,i){
	    return el/del0.e(i)
	}).max())
	ratio = Math.min(ratio, 1000000)
	if(ratio <= 1){
	    // Error too large; decrease h and try again
	    h = S*h*Math.pow(ratio,0.25)
	}
	else{
	    t += h
	    x = x.add(inc)
	    vals[idx] = [t, x.e(1)]
	    idx++
	    h = S*h*Math.pow(ratio,0.2)
	}
    }
    return vals
}
